/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Heriot-Watt University, UK.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Heriot-Watt University nor the names of
*     its contributors may be used to endorse or promote products
*     derived from this software without specific prior written
*     permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
*  Author: Joel Cartwright
*
*********************************************************************/

#include <iostream>
#include <sstream>
using namespace std;

#include <ros/ros.h>
#include <osl_core/ModuleCore.h>
#include <auv_msgs/WorldWaypointReq.h>

// ---------------------------------------------------------------------------
void printUsage(const std::string &progName)
{
	cout << endl;
	cout << "usage:" << endl;
	cout << "  " << progName << " depth|altitude n e d|a pitch yaw" << endl;
	cout << "  depth or altitude requests either depth or altitude control of the z axis." << endl;
	cout << "  If 'n' is given for an axis value, control will be disabled for that axis." << endl;
	cout << "  North and east must either both be enabled or both disabled, as they" << endl;
	cout << "  affect x and y together." << endl;
	cout << endl;
}

// ---------------------------------------------------------------------------
int main(int argc, char **argv)
{
	string progName = basename(argv[0]);
	string nodeName = osl_core::ModuleCore::createROSNodeName(progName);

	// Work from end of argv, ignoring special arguments (for ROS) that
	// begin '__'.
	for (int i=argc-1; i>=1; --i) {
		char *p = strstr(argv[i], "__");
		if (p != NULL && p == argv[i]) {
			cout << "Ignoring special ROS arg " << i << ": " << argv[i];
			argc--;
		}
	}

	if (argc != 7) {
		if (argc != 1) cout << "Invalid arguments." << endl;
		for (int i=1; i<argc; ++i) {
			cout << "  arg " << i << ": " << argv[i] << endl;
		}
		printUsage(progName);
		exit(1);
	}

	auv_msgs::WorldWaypointReq msg;

	msg.goal.requester = nodeName;
	// Goal ID 1 is fine, as nodeName contains PID so will change each time we run.
	msg.goal.id = 1;
	msg.goal.priority = auv_msgs::GoalDescriptor::PRIORITY_NORMAL;

	int argPos = 1;
	if (argPos < argc && strcmp(argv[argPos], "depth") == 0) {
		msg.altitude_mode = false;
	}
	else if (argPos < argc && strcmp(argv[argPos], "altitude") == 0) {
		msg.altitude_mode = true;
	}
	else {
		cerr << "Invalid first parameter: " << argv[1] << endl;
		printUsage(progName);
		exit(1);
	}
	++argPos;

	if (strcmp(argv[argPos], "n") == 0) msg.disable_axis.x = true;
	else msg.position.north = atof(argv[argPos]);
	++argPos;

	if (strcmp(argv[argPos], "n") == 0) msg.disable_axis.y = true;
	else msg.position.east = atof(argv[argPos]);
	++argPos;

	if ((msg.disable_axis.x || msg.disable_axis.y) && !(msg.disable_axis.x && msg.disable_axis.y)) {
		cerr << "North and east values must either both be valid, or both 'n'." << endl;
		printUsage(progName);
		exit(1);
	}

	if (strcmp(argv[argPos], "n") == 0) msg.disable_axis.z = true;
	else {
		if (msg.altitude_mode) {
			msg.altitude = atof(argv[argPos]);
		}
		else {
			msg.position.depth = atof(argv[argPos]);
		}
	}
	++argPos;

	if (strcmp(argv[argPos], "n") == 0) msg.disable_axis.pitch = true;
	else msg.orientation.pitch = osl_core::deg2rad(atof(argv[argPos]));
	++argPos;

	if (strcmp(argv[argPos], "n") == 0) msg.disable_axis.yaw = true;
	else msg.orientation.yaw = osl_core::deg2rad(atof(argv[argPos]));
	++argPos;

	ros::init(argc, argv, nodeName);
	ros::NodeHandle node;

	ros::Publisher waypointPub = node.advertise<auv_msgs::WorldWaypointReq>("pilot/world_waypoint_req", 1);
	int rate = 5;

	cout << msg;

	cout << "Program arguments use degrees, which are converted to radians for message." << endl;
	cout << "Sending world waypoint request at " << rate << " Hz. Press Ctrl-C to stop." << endl;

	ros::Rate loop_rate(rate);

	while (ros::ok())
	{
		msg.header.stamp = ros::Time::now();
		waypointPub.publish(msg);

		ros::spinOnce();
		loop_rate.sleep();
	}

	return 0;
}
